#include "ros_usbcapture/RosUsbCapture.h"

namespace wmj
{
    camera_node::camera_node(const rclcpp::NodeOptions &options) : rclcpp::Node("camera_node", options)
    {
        RCLCPP_INFO(this->get_logger(), "camera_node init working ...");
        init = false;

        // 初始化相机

        RCLCPP_INFO(this->get_logger(), "camera_node init working ...");
        m_capture = std::make_shared<wmj::UsbCaptureSystem>(USBCAPTURE_CFG); // USBCAPTURE_CFG

        auto Function = std::bind(&camera_node::RosImageCallBack, this,std::placeholders::_1,std::placeholders::_2);
        m_capture->setPubPtr(Function);

        // 初始化相机
        if(m_capture->m_debug == 1){
            // m_capture = std::make_shared<wmj::UsbCaptureSystem>(); // USBCAPTURE_CFG
            m_capture->blindOpenDevice();
            m_capture->getCameraInfo();
            // m_capture->displayAllCameras();
        }
        else{
            m_camera_num = m_capture->activeCameraCount();
            m_capture->cameraMode("armor");
            m_capture->getCameraInfo();
        }
        std::cout << "Camera number going to open:" << m_camera_num << std::endl;
        std::cout << "Active camera num: " << m_camera_num << std::endl;

        // 单双目发布者构造
        rclcpp::QoS m_qos = setQos(true, m_camera_num);
        // Image[m_camera_num];
        Camera[m_camera_num];
        Cim[m_camera_num];
        m_frames.resize(m_camera_num);
        cv::FileStorage fs(USBCAPTURE_CFG, cv::FileStorage::READ);
        int i = 0;
        for (auto node : fs["cameras"])
        {
            std::string camera_name = node["orientation"];
            m_frames[i].m_orientation = camera_name + "_camera_frame";
            // m_frames[i].m_orientation = "right_camera_frame";
            auto ImgPub = this->create_publisher<sensor_msgs::msg::Image>(camera_name + "_image_raw", m_qos);
            imageMapping[m_frames[i].m_orientation] = ImgPub;
            // Image.push_back(this->create_publisher<sensor_msgs::msg::Image>(camera_name + "_image_raw", m_qos));
            Camera.push_back(this->create_publisher<sensor_msgs::msg::CameraInfo>("camera_info_" + camera_name , m_qos));
            std::string path = "package://ros_usbcapture/config/" + camera_name + ".yaml";
            Cim.push_back(std::make_shared<camera_info_manager::CameraInfoManager>(this, "camera_info_" + camera_name, path));
            i++;
        }
        init = true;

        // 创建发布者
        // timer = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&camera_node::timer_callback, this));
        RCLCPP_INFO(this->get_logger(), "detector_component init successfully ");
    }

    void camera_node::timer_callback()
    {
        // for (int i = 0; i < m_camera_num; i++)
        // {
        //     if ((*m_capture >> m_frames[i]) != 0)
        //     {
        //         std::cout << "get images wrong!!!\n";
        //         m_capture->OfflineDeviceCallback();
        //     }
        //     if (m_frames[i].m_img.empty())
        //     {
        //         cv::Mat empty(1024, 1280, CV_8UC3, cv::Scalar::all(0));
        //         m_frames[i].m_img = empty;
        //         cv::putText(m_frames[i].m_img, "No Image", cv::Point(400, 400), cv::FONT_HERSHEY_COMPLEX, 3, cv::Scalar::all(255), 1);
        //         cv::putText(m_frames[i].m_img, "This m_frames", cv::Point(350, 500), cv::FONT_HERSHEY_COMPLEX, 3, cv::Scalar::all(255), 1);
        //         cv::putText(m_frames[i].m_img, "is empty!", cv::Point(350, 600), cv::FONT_HERSHEY_COMPLEX, 3, cv::Scalar::all(255), 1);
        //         std::cout << "############################  No image!!!  ##############################" << std::endl;
                
        //     }
        //     m_camera_num = m_capture->activeCameraCount();
        //     sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());
        //     auto img_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", m_frames[i].m_img);
        //     *msg = *img_msg.toImageMsg();
        //     msg->header.frame_id = m_frames[i].m_orientation;
        //     builtin_interfaces::msg::Time t;
        //     t.sec = (int) m_frames[i].m_time_stamp;
        //     t.nanosec =  (m_frames[i].m_time_stamp - (int) m_frames[i].m_time_stamp)* 1e9 ;
        //     msg->header.stamp =t;
        //     // std::cout << "msg time: " << std::setprecision(9) << (msg->header.stamp.sec+msg->header.stamp.nanosec/1e9) - m_frames[i].m_time_stamp << " img time: " << std::endl;
        //     Image[i]->publish(std::move(msg));
        //     Camera[i]->publish(Cim[i]->getCameraInfo());
        // }

    }
    // 设置传输qos
    rclcpp::QoS camera_node::setQos(bool if_reliable, int depth)
    {
        rmw_qos_profile_t qos_t = setQos_t(if_reliable, depth);
        rclcpp::QoSInitialization qosInitialization = rclcpp::QoSInitialization::from_rmw(qos_t);
        rclcpp::QoS qos = rclcpp::QoS(qosInitialization, qos_t);
        return qos;
    }

    // 设置传输qos_t
    rmw_qos_profile_t camera_node::setQos_t(bool if_reliable, int depth)
    {
        rmw_qos_profile_t qos_t;
        if (if_reliable)
        {
            std::cout << "[UCS]reliable_image" << std::endl;
            qos_t.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
        }
        else
        {
            std::cout << "[UCS]best_effort_image" << std::endl;
            qos_t.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
        }
        qos_t.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
        qos_t.depth = depth;
        qos_t.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
        qos_t.deadline = RMW_QOS_DEADLINE_DEFAULT;
        qos_t.lifespan = RMW_QOS_LIFESPAN_DEFAULT;
        qos_t.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
        qos_t.liveliness_lease_duration = RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT;
        qos_t.avoid_ros_namespace_conventions = false;

        return qos_t;
    }

    cv::Mat camera_node::GetImg(sensor_msgs::msg::Image img)
    {
        // 声明 cv::image 指针，内含cv::Mat
        cv_bridge::CvImageConstPtr cv_ptr;
        // toCvShare() 使用
        // sensor_msgs::msg::Image::ConstPtr img_ptr;

        // 获取图像，最终图像通过 cv_ptr->image 获取，为cv::Mat型
        try
        {
            // 把msg里的 Image 转换为 cv::mat类型 , 有两种方式 toCvCopy() 和 toCvShare() ,toCvShare() 虽然可以实现直接引用不用拷贝，
            // 但需要创建 sensors_msgs::ImageConst 指针，经测试二者差别不大，故用toCvCopy().后续还可改进.

            cv_ptr = cv_bridge::toCvCopy(img);

            // toCvShare() 方法
            // img_ptr = std::make_shared<const sensor_msgs::msg::Image>(msg->image);
            // cv_ptr = cv_bridge::toCvCopy(img_ptr);
        }
        catch (cv_bridge::Exception &e)
        {
            printf("cv_bridge exception:%s", e.what());
        }
        return (cv_ptr->image);   
    }
    void camera_node::RosImageCallBack(wmj::MatWithTime& raw_img,std::string orientation)
    {
        sensor_msgs::msg::Image::UniquePtr msg(new sensor_msgs::msg::Image());
        auto img_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", raw_img.m_img);
        *msg = *img_msg.toImageMsg();
        msg->header.frame_id = orientation;
        builtin_interfaces::msg::Time t;
        t.sec = (int) raw_img.m_time_stamp;
        t.nanosec =  (raw_img.m_time_stamp - (int) raw_img.m_time_stamp)* 1e9 ;
        msg->header.stamp =t;
        if(init)
        {
            imageMapping[orientation]->publish(*msg);
            // Camera[0]->publish(Cim[0]->getCameraInfo());
        }
        else
        {
            std::cout << "WAIT FOR INIT" << std::endl;
        }

        return;
    }
}


#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(wmj::camera_node)